14. Generating Sigma Points Assignment 1

Assignment Sigma Point Generation

Instructor Notes:

In the video, the upper left value of sqrt(P_k|k) is 0.00656. It should actually be 0.0656

Generating Sigma Points Assignment

Task Description:

You will be completing the missing code in ukf.cpp, GenerateSigmaPoints() function.

Task List:

Task Feedback:

Nice! Have you tested your code below?

Helpful Equations and Resources

X_{k|k} = \Bigg [ x_{k|k} \qquad x_{k|k}+\sqrt{(\lambda+n_x)P_{k|k}} \qquad x_{k|k}-\sqrt{(\lambda+n_x)P_{k|k}} \Bigg]

remember that \large x_{k|k} is the first column of the Sigma matrix.

\large x_{k|k}+\sqrt{(\lambda+n_x)P_{k|k}} is the second through \large n_x +1 column.

\large x_{k|k}-\sqrt{(\lambda+n_x)P_{k|k}} is the \large n_x +2 column through \large 2n_x +1 column.

and you can set columns easily in a matrix using the .col() function.

Please note that the algorithm used in the quiz (P.llt().matrixL()) produces the lower triangular matrix L of the matrix P such that P = L*L^ .

Start Quiz: 

#include <iostream>
#include "Dense"
#include "ukf.h"

using Eigen::MatrixXd;

int main() {

  // Create a UKF instance
  UKF ukf;

  /**
   * Programming assignment calls
   */
  MatrixXd Xsig = MatrixXd(5, 11);
  ukf.GenerateSigmaPoints(&Xsig);

  // print result
  std::cout << "Xsig = " << std::endl << Xsig << std::endl;

  return 0;
}
#include "ukf.h"
#include <iostream>

using Eigen::MatrixXd;
using Eigen::VectorXd;

UKF::UKF() {
  Init();
}

UKF::~UKF() {

}

void UKF::Init() {

}

/**
 * Programming assignment functions: 
 */
void UKF::GenerateSigmaPoints(MatrixXd* Xsig_out) {

  // set state dimension
  int n_x = 5;

  // define spreading parameter
  double lambda = 3 - n_x;

  // set example state
  VectorXd x = VectorXd(n_x);
  x <<   5.7441,
         1.3800,
         2.2049,
         0.5015,
         0.3528;

  // set example covariance matrix
  MatrixXd P = MatrixXd(n_x, n_x);
  P <<     0.0043,   -0.0013,    0.0030,   -0.0022,   -0.0020,
          -0.0013,    0.0077,    0.0011,    0.0071,    0.0060,
           0.0030,    0.0011,    0.0054,    0.0007,    0.0008,
          -0.0022,    0.0071,    0.0007,    0.0098,    0.0100,
          -0.0020,    0.0060,    0.0008,    0.0100,    0.0123;

  // create sigma point matrix
  MatrixXd Xsig = MatrixXd(n_x, 2 * n_x + 1);

  // calculate square root of P
  MatrixXd A = P.llt().matrixL();

  /**
   * Student part begin
   */

  // your code goes here 
  // calculate sigma points ...

  // set sigma points as columns of matrix Xsig

  /**
   * Student part end
   */

  // print result
  // std::cout << "Xsig = " << std::endl << Xsig << std::endl;

  // write result
  *Xsig_out = Xsig;
}

/**
 * expected result:
 * Xsig =
 *  5.7441  5.85768   5.7441   5.7441   5.7441   5.7441  5.63052   5.7441   5.7441   5.7441   5.7441
 *    1.38  1.34566  1.52806     1.38     1.38     1.38  1.41434  1.23194     1.38     1.38     1.38
 *  2.2049  2.28414  2.24557  2.29582   2.2049   2.2049  2.12566  2.16423  2.11398   2.2049   2.2049
 *  0.5015  0.44339 0.631886 0.516923 0.595227   0.5015  0.55961 0.371114 0.486077 0.407773   0.5015
 *  0.3528 0.299973 0.462123 0.376339  0.48417 0.418721 0.405627 0.243477 0.329261  0.22143 0.286879
 */
#ifndef UKF_H
#define UKF_H

#include "Dense"

class UKF {
 public:
  /**
   * Constructor
   */
  UKF();

  /**
   * Destructor
   */
  virtual ~UKF();

  /**
   * Init Initializes Unscented Kalman filter
   */
  void Init();

  /**
   * Student assignment functions
   */
  void GenerateSigmaPoints(Eigen::MatrixXd* Xsig_out);
  void AugmentedSigmaPoints(Eigen::MatrixXd* Xsig_out);
  void SigmaPointPrediction(Eigen::MatrixXd* Xsig_out);
  void PredictMeanAndCovariance(Eigen::VectorXd* x_pred, 
                                Eigen::MatrixXd* P_pred);
  void PredictRadarMeasurement(Eigen::VectorXd* z_out, 
                               Eigen::MatrixXd* S_out);
  void UpdateState(Eigen::VectorXd* x_out, 
                   Eigen::MatrixXd* P_out);
};

#endif  // UKF_H

expected result:

Xsig =

5.7441 5.85768 5.7441 5.7441 5.7441 5.7441 5.63052 5.7441 5.7441 5.7441 5.7441

1.38 1.34566 1.52806 1.38 1.38 1.38 1.41434 1.23194 1.38 1.38 1.38

2.2049 2.28414 2.24557 2.29582 2.2049 2.2049 2.12566 2.16423 2.11398 2.2049 2.2049

0.5015 0.44339 0.631886 0.516923 0.595227 0.5015 0.55961 0.371114 0.486077 0.407773 0.5015

0.3528 0.299973 0.462123 0.376339 0.48417 0.418721 0.405627 0.243477 0.329261 0.22143 0.286879

Next Concept